#include <string.h>
#include "bsp_abs_encoder.h"
#include "bsp_spi_bus.h"

#define CMD_STREAM_MODE 0x05 /*TDK传感器操作码*/ //Streaming mode format
#define CMD_DUMMY       0xff

#define JOINT_ENCODER_ID 1
#define PUSHROD_ENCODER_ID 2

typedef enum 
{
    CS_SELECT = 0,
    CS_DISSELECT,
}CS_sta_e;

typedef struct TDK_encoder
{
    uint16_t raw_data;
    uint8_t status;
    uint8_t crc;
} TDK_encoder_t;

TDK_encoder_t joint_abs_encoder, pushrod_encoder;


/**
 * @brief 绝对式编码器片选控制函数
 * 
 * @param level 取值0-1。选中:0;不选中:1
 * @param spi_id spi索引,取值范围 0 到 SPI_TOTAL_NUM - 1 
 */
static void absEnc_setCS(CS_sta_e cs_status, uint8_t spi_id, uint8_t device_id)
{
    if (cs_status == CS_SELECT)
    {
        // bsp_InitSPIParam(SPI_BAUDRATEPRESCALER_2, SPI_PHASE_1EDGE, SPI_POLARITY_LOW);
        bsp_spiBusEnter(spi_id, device_id);
    }
    else
    {
        bsp_spiBusExit(spi_id, device_id);
    }
}

/*读取单圈编码器原始值*/
static void TdkEnc_SetstreamModeAndReadData(TDK_encoder_t *p_abs_encoder, uint8_t spi_id, uint8_t device_id)
{
    uint8_t index = bsp_getSpiIndex(spi_id);
    if (NULL == p_abs_encoder)
        return;

    /*SPI CMD_WRITE_REG*/ // 拉低片选
    absEnc_setCS(CS_SELECT, spi_id, device_id);

    g_spi_ctrl[index].data_len = 0;

    //TDK传感器操作码,Streaming mode format，绝对式编码器设置为流模式并获取后4个字节的数据
    g_spi_ctrl[index].tx_buf[0] = CMD_STREAM_MODE; 
    memset((void*)&g_spi_ctrl[index].tx_buf[1], CMD_DUMMY, 4);
    g_spi_ctrl[index].data_len = 5;
    bsp_spiTransfer(spi_id);

    /*获取角度原始数据*/
    p_abs_encoder->raw_data = ((uint16_t)g_spi_ctrl[index].rx_buf[1]) << 8 | g_spi_ctrl[index].rx_buf[2];

    /*获取状态和CRC数据*/
    p_abs_encoder->status = g_spi_ctrl[index].rx_buf[3];
    p_abs_encoder->crc = g_spi_ctrl[index].rx_buf[4];

    absEnc_setCS(CS_DISSELECT, spi_id, device_id); // 拉高片选
}

/**
 * @brief 更新绝对式编码器数据
 * 
 */
void updateAbsEncoderData(void)
{
    TdkEnc_SetstreamModeAndReadData(&joint_abs_encoder, ENC_SPI_INDEX, JOINT_ENCODER_ID);   // 关节编码器
    TdkEnc_SetstreamModeAndReadData(&pushrod_encoder, ENC_SPI_INDEX, PUSHROD_ENCODER_ID);    // 推杆编码器
}

/**
 * @brief get joint absolute encoder status Word
 * 
 * @return uint8_t joint absolute encoder status Word
 */
uint8_t getJointAbsEncoderStatusWord(void)
{
    return joint_abs_encoder.status;
}

/**
 * @brief get joint absolute encoder angel raw data
 * 
 * @return uint16_t The encoder is 16bit, data is from 0~65535
 */
uint16_t getJointAbsEncoderRawData(void)
{
    return joint_abs_encoder.raw_data;
}

/**
 * @brief get pushrod absolute encoder status Word
 * 
 * @return uint8_t absolute encoder status Word
 */
uint8_t getPushrodEncoderStatusWord(void)
{
    return pushrod_encoder.status;
}

/**
 * @brief get absolute encoder angel raw data
 * 
 * @return uint16_t The encoder is 16bit, data is from 0~65535
 */
uint16_t getPushrodEncoderRawData(void)
{
    return pushrod_encoder.raw_data;
}
